#include "Main.h"
#include "Ball.h"
#include "Fizyka.h"
#include "Menu.h";
const int stanmenu = 0;
const int stangra = 20;
const int stankoniec = 10;
const int stankalibracja = 1;
const int stanrekord = 2;
const int stanoautorach = 3;
using namespace cv;
Fizyka pilki;

IplImage *image[4] = { 0, 0, 0, 0 }, *hsv[4] = { 0, 0, 0, 0 }, *hue[4] = { 0,
		0, 0, 0 }, *mask[4] = { 0, 0, 0, 0 }, *backproject[4] = { 0, 0, 0, 0 };
CvHistogram *hist[4] = { 0, 0, 0, 0 };

int skala = 2;
double imgWidth, imgHeight;

int select_object[4] = { 0, 0, 0, 0 };
int track_object[4] = { 0, 0, 0, 0 };

CvPoint origin[4];
CvRect selection[4];
CvRect track_window[4];
CvBox2D track_box[4];
CvConnectedComp track_comp[4];
int hdims = 16;
float hranges_arr[] = { 0, 180 };
float* hranges = hranges_arr;
int vmin = 10, vmax = 256, smin = 180;
bool wyjdz = false, rekord = false, oAutorach = false;

void mouseHandler(int event, int ax, int ay, int flags, void* param);
void onSpeedX(int position);
void onSpeedY(int position);
int getWidth() {
	return (int) cvGetCaptureProperty(captureDevice, CV_CAP_PROP_FRAME_WIDTH);
}

int getHeight() {
	return (int) cvGetCaptureProperty(captureDevice, CV_CAP_PROP_FRAME_HEIGHT);
}
int spedx, spedy;
void initDisplayWindow(bool calibrate) {
	cvNamedWindow("okno", CV_WINDOW_AUTOSIZE);
	//cvNamedWindow("dev", CV_WINDOW_AUTOSIZE);
	if (calibrate) {
		cvCreateTrackbar("Brightness", "okno", &brightness, 100,
				onBrightnessCalibrated);
		cvCreateTrackbar("Threshold", "okno", &kneeThreshold, 200,
				onThresholdCalibrated);
		cvCreateTrackbar("Detection", "okno", &smin, 200, NULL);
		cvCreateTrackbar("Marker", "okno", &nrZnacznika, 3, onZnacznik);
		//cvCreateTrackbar("Speedx", "okno", &spedx, 255, onSpeedX);
		//cvCreateTrackbar("Speedy", "okno", &spedy, 255, onSpeedY);

	}
}

bool initVideoCapture(int brightness, int width = 640, int height = 480) {
	captureDevice = cvCreateCameraCapture(CV_CAP_ANY);
	cvSetCaptureProperty(captureDevice, CV_CAP_PROP_BRIGHTNESS, brightness
			/ 100.0);
	/*cvSetCaptureProperty(captureDevice, CV_CAP_PROP_FRAME_HEIGHT, width);
	 cvSetCaptureProperty(captureDevice, CV_CAP_PROP_FRAME_WIDTH, height);
	 */
	return captureDevice != NULL;
}

void processFrames() {

	cvSetMouseCallback("okno", mouseHandler, 0);
	IplImage *dst;
	IplImage *dst2;
	dst2 = NULL;
	for (;;) {

		lastFrame = cvQueryFrame(captureDevice);
		if (!lastFrame) {
			break;
		}
		cvFlip(lastFrame, NULL, 1);
		if (oAutorach) {
			//printf("tu1");
			//fflush(stdout);

			dst = cvCreateImage(cvSize(lastFrame->width * skala,
					lastFrame->height * skala), lastFrame->depth,
					lastFrame->nChannels);

			cvResize(lastFrame, dst);

			*lastFrame = *dst;
			oAutorachDisplay(lastFrame, pilki, imgWidth * skala, imgHeight
					* skala);
			cvShowImage("okno", lastFrame);
			//if (dst2 != NULL)
			//	cvReleaseImage(&dst2);
			dst2 = dst;
			char ch = cvWaitKey(2);
			if (ch == 27 || wyjdz)
				break;
			continue;
		}
		if (pilki.stan == stankoniec) {
			//printf("tu2");
			//			fflush(stdout);
			dst = cvCreateImage(cvSize(lastFrame->width * skala,
					lastFrame->height * skala), lastFrame->depth,
					lastFrame->nChannels);
			cvResize(lastFrame, dst);
			*lastFrame = *dst;
			displayscore(lastFrame, pilki);
			cvShowImage("okno", lastFrame);
			//if (dst2 != NULL)
			//	cvReleaseImage(&dst2);
			dst2 = dst;
			char ch = cvWaitKey(2);
			if (ch == 27 || wyjdz)
				break;
			continue;

		}
		if (rekord) {
			//printf("tu3");
			//			fflush(stdout);
			dst = cvCreateImage(cvSize(lastFrame->width * skala,
					lastFrame->height * skala), lastFrame->depth,
					lastFrame->nChannels);
			cvResize(lastFrame, dst);
			*lastFrame = *dst;
			rekordDisplay(lastFrame, pilki, imgWidth * skala, imgHeight * skala);
			cvShowImage("okno", lastFrame);
			//if (dst2 != NULL)
			//	cvReleaseImage(&dst2);
			dst2 = dst;
			char ch = cvWaitKey(2);
			if (ch == 27 || wyjdz)
				break;
			continue;
		}
		if (pilki.stan == stanmenu) {
			//printf("tu4");fflush(stdout);
			dst = cvCreateImage(cvSize(lastFrame->width * skala,
					lastFrame->height * skala), lastFrame->depth,
					lastFrame->nChannels);
			cvResize(lastFrame, dst);
			*lastFrame = *dst;
			menu(lastFrame, pilki, imgWidth * skala, imgHeight * skala);
			cvShowImage("okno", lastFrame);
			//if (dst2 != NULL)
			//	cvReleaseImage(&dst2);
			dst2 = dst;
			char ch = cvWaitKey(2);
			if (ch == 27 || wyjdz)
				break;
			continue;
		}

		for (int i = 0; i < 4; i++) {
			//printf("tu5");fflush(stdout);
			if (!image[i]) {
				/* allocate all the buffers */
				image[i] = cvCreateImage(cvGetSize(lastFrame), 8, 3);
				image[i]->origin = lastFrame->origin;
				hsv[i] = cvCreateImage(cvGetSize(lastFrame), 8, 3);
				hue[i] = cvCreateImage(cvGetSize(lastFrame), 8, 1);
				mask[i] = cvCreateImage(cvGetSize(lastFrame), 8, 1);
				backproject[i] = cvCreateImage(cvGetSize(lastFrame), 8, 1);
				hist[i] = cvCreateHist(1, &hdims, CV_HIST_ARRAY, &hranges, 1);
				//histimg = cvCreateImage(cvSize(320, 200), 8, 3);
				//cvZero(histimg);
			}

			cvCopy(lastFrame, image[i], 0);
			cvCvtColor(image[i], hsv[i], CV_BGR2HSV);
			if (track_object[i]) {
				int _vmin = vmin, _vmax = vmax;

				cvInRangeS(hsv[i], cvScalar(0, smin, MIN(_vmin,_vmax), 0),
						cvScalar(180, 256, MAX(_vmin,_vmax), 0), mask[i]);
				cvSplit(hsv[i], hue[i], 0, 0, 0);

				// oblicznie koloru do sledzenia
				if (track_object[i] < 0) {
					float max_val = 0.f;
					cvSetImageROI(hue[i], selection[i]);
					cvSetImageROI(mask[i], selection[i]);
					cvCalcHist(&hue[i], hist[i], 0, mask[i]);
					cvGetMinMaxHistValue(hist[i], 0, &max_val, 0, 0);
					cvConvertScale(hist[i]->bins, hist[i]->bins, max_val ? 255.
							/ max_val : 0., 0);
					cvResetImageROI(hue[i]);
					cvResetImageROI(mask[i]);
					track_window[i] = selection[i];
					track_object[i] = 1;

				}

				cvCalcBackProject( &hue[i], backproject[i], hist[i] );
				cvAnd(backproject[i], mask[i], backproject[i], 0);
				cvCamShift(backproject[i], track_window[i], cvTermCriteria(
						CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1),
						&track_comp[i], &track_box[i]);
				track_window[i] = track_comp[i].rect;

				if (!image[i]->origin)
					track_box[i].angle = -track_box[i].angle;
				//cvEllipseBox(image[i], track_box[i], CV_RGB(255,0,0), 1, CV_AA, 0);
				if (true)//!pilki.gamePlay && pilki.gameView)
					cvEllipseBox(lastFrame, track_box[i], CV_RGB(255,0,0), 1,
							CV_AA, 0);
			}
			if (select_object[i] && selection[i].width > 0
					&& selection[i].height > 0) {
				cvSetImageROI(lastFrame, selection[i]);
				cvXorS(lastFrame, cvScalarAll(255), lastFrame, 0);
				cvResetImageROI(lastFrame);
			}
		}
		dst = cvCreateImage(cvSize(lastFrame->width * skala, lastFrame->height
				* skala), lastFrame->depth, lastFrame->nChannels);
		cvResize(lastFrame, dst);

		*lastFrame = *dst;
		pilki.posX = track_box[0].center.x * skala;
		pilki.posY = track_box[0].center.y * skala;
		if (pilki.stan == stangra)
			pilki.draw(lastFrame, lastFrame->height, lastFrame->width,
					track_box);
		if (pilki.stan == stankoniec) {
			pilki.counter = pilki.score;
		}
		cvRectangle(
				lastFrame,
				cvPoint(0, lastFrame->height - kneeThreshold),
				cvPoint(lastFrame->width, lastFrame->height - kneeThreshold + 1),
				colors[5]);
		if (pilki.stan == stankalibracja) {
			viewDisplay(lastFrame, pilki, imgWidth * skala, imgHeight * skala);
		}
		cvShowImage("okno", lastFrame);
		//if (dst2 != NULL)
		//	cvReleaseImage(&dst2);
		dst2 = dst;
		char ch = cvWaitKey(2);
		if (ch == 27)
			break;

	}
}

void releaseDisplayWindow() {
	cvDestroyWindow("okno");
}

void releaseVideoCapture() {
	if (captureDevice)
		cvReleaseCapture(&captureDevice);
}

void onBrightnessCalibrated(int position) {
	cvSetCaptureProperty(captureDevice, CV_CAP_PROP_BRIGHTNESS, position
			/ 100.0);
}

void onZnacznik(int position) {
	nrZnacznika = position;
}

void onThresholdCalibrated(int position) {
	kneeThreshold = position;
}

void onSpeedX(int position) {
	markerColour.val[0] = position;
}
void onSpeedY(int position) {
	markerColour.val[0] = position;
}
void mouseHandler(int event, int ax, int ay, int flags, void* param) {
	if (event == CV_EVENT_MOUSEMOVE) {
		if (pilki.stan < 20) {
			pilki.x = ax;
			pilki.y = ay;
		}

	}
	if (event == 7) {

		pilki.x = imgWidth * skala * 0.5;
		pilki.y = imgHeight * skala * 0.2;
		pilki.speedX = 0;
		pilki.speedY = 0;
	}

	if (pilki.stan == stanmenu) {
		if (event == CV_EVENT_LBUTTONUP) {
			if (pilki.y > (imgHeight * skala / 5) * 4 - 55) {
				if (!oAutorach && !rekord)
					wyjdz = true;
				oAutorach = false;
				rekord = false;
			} else if (pilki.y > (imgHeight * skala / 5) * 3 - 55) {
				oAutorach = true;
			} else if (pilki.y > (imgHeight * skala / 5) * 2 - 55) {
				rekord = true;
			} else {
				select_object[0] = 0;
				select_object[1] = 0;
				select_object[2] = 0;
				select_object[3] = 0;
				pilki.stan = stankalibracja;
				pilki.counter = 0;
				pilki.dzielnik=0;
				track_object[0] = 0;
				track_object[1] = 0;
				track_object[2] = 0;
				track_object[3] = 0;
			}

		}
	}
	if (event == CV_EVENT_MOUSEMOVE && pilki.stan == stankoniec) {
		if (ay > ((imgHeight * skala) / 4) * 3 - 30)
			pilki.score = -2;
		else
			pilki.score = -3;
	}

	if (pilki.stan == stankalibracja) {
		pilki.mx = ax;
		pilki.my = ay;
		if (event == CV_EVENT_LBUTTONUP) {
			if (ax > (imgWidth * skala / 10) * 8 && ay < 45) {
				pilki.stan = stangra;
				pilki.x = imgWidth * skala * 0.5;
				pilki.y = imgHeight * skala * 0.2;
				pilki.speedX = 0;
				pilki.speedY = 0;
				pilki.score = 0;
			}
		}
		if (event == CV_EVENT_MOUSEMOVE&&pilki.dzielnik==0) {
			pilki.counter=0;
			for (int i = 1; i < 6; i++) {
				if (ay < imgHeight * skala / 2 - 65 && ay > imgHeight * skala
						/ 2 - 90)
					if (ax > imgWidth * skala / 7 * i-5 && ax < imgWidth * skala
							/ 7 * i + 25) {
						pilki.counter = i;

					}
			}
		}
		if (event ==CV_EVENT_LBUTTONUP && pilki.dzielnik==0) {
					pilki.counter=0;
					for (int i = 1; i < 6; i++) {
						if (ay < imgHeight * skala / 2 - 65 && ay > imgHeight * skala
								/ 2 - 90)
							if (ax > imgWidth * skala / 7 * i-5 && ax < imgWidth * skala
									/ 7 * i + 25) {
								pilki.dzielnik = i;
								pilki.counter=0;
							}
					}
				}
	}
	if (pilki.stan == stankalibracja) {
		if (select_object[nrZnacznika]) {

			selection[nrZnacznika].x = MIN(ax/skala,origin[nrZnacznika].x);
			selection[nrZnacznika].y = MIN(ay/skala,origin[nrZnacznika].y);
			selection[nrZnacznika].width = selection[nrZnacznika].x
					+ CV_IABS(ax/skala - origin[nrZnacznika].x);
			selection[nrZnacznika].height = selection[nrZnacznika].y
					+ CV_IABS(ay/skala - origin[nrZnacznika].y);

			selection[nrZnacznika].x = MAX( selection[nrZnacznika].x, 0 );
			selection[nrZnacznika].y = MAX( selection[nrZnacznika].y, 0 );
			selection[nrZnacznika].width
					= MIN( selection[nrZnacznika].width, image[nrZnacznika]->width );
			selection[nrZnacznika].height
					= MIN( selection[nrZnacznika].height, image[nrZnacznika]->height );
			selection[nrZnacznika].width -= selection[nrZnacznika].x;
			selection[nrZnacznika].height -= selection[nrZnacznika].y;
		}

		switch (event) {
		case CV_EVENT_LBUTTONDOWN:
			origin[nrZnacznika] = cvPoint(ax / skala, ay / skala);
			selection[nrZnacznika] = cvRect(ax / skala, ay / skala, 0, 0);
			select_object[nrZnacznika] = 1;

			break;
		case CV_EVENT_LBUTTONUP:
			select_object[nrZnacznika] = 0;
			if (selection[nrZnacznika].width > 0
					&& selection[nrZnacznika].height > 0)
				track_object[nrZnacznika] = -1;
			break;
		}
	}
	if (event == CV_EVENT_LBUTTONUP && pilki.stan == stankoniec) {
		if (ay > ((imgHeight * skala) / 4) * 3 - 30) {
			pilki.stan = stanmenu;
			pilki.score = 0;
		} else if (ay < ((imgHeight * skala) / 4)) {
			pilki.score = 0;
			pilki.x = imgWidth * skala * 0.5;
			pilki.y = imgHeight * skala * 0.2;
			pilki.speedX = 0;
			pilki.speedY = 0;
			pilki.stan = stangra;
		}
	}
}

int main() {
	rekord = 0;
	initDisplayWindow(true);
	initVideoCapture(brightness);
	lastFrame = cvQueryFrame(captureDevice);
	imgWidth = lastFrame->width;
	imgHeight = lastFrame->height;
	if (imgWidth > 400)
		skala = 1;
	pilki = Fizyka(200, 40, 5, skala);
	processFrames();

	releaseVideoCapture();
	releaseDisplayWindow();

}
